﻿using System;
using System.Collections.Generic;
using System.Drawing;
using System.IO;
using System.Media;
using System.Net;
using System.Net.Sockets;
using System.Text;
using System.Threading;
using System.Windows.Forms;
using com.atiia.automation.sensors;
using ControlSystem;
using com.motoman.UDP;
using System.ComponentModel;
using SenrobControl;
using SenrobCapture;
using AForge.Video;

namespace SenRobCSTest
{
    public partial class MainForm : Form
    {
        enum SDA10Arm { Left, Right };
        SDA10Arm current_arm = SDA10Arm.Right;

        enum FTactivity { Haptic, Joystick, None };
        FTactivity current_ft = FTactivity.Joystick;

        enum Activity { None, Experiment, FT };
        Activity current_act = Activity.None;

        int current_exp = 0;
        FTactivity current_exp_type = FTactivity.Joystick;

        Form TrackpadWindow;

        bool logging = false;

        public bool IsLogging() { return logging; }
        List<string> ftlogfile, explogfile, logfile, pingfile, mousefile;
        string ftlogname = "";
        string explogname = "";
        int ftlognum = 0;
        int explognum = 0;
        Point current_mouse_pos = new Point();
        private string GetDateTimeString(DateTime dt)
        {
            return dt.Hour + "." + dt.Minute + "." + dt.Second + "." + dt.Millisecond;
        }
        void StartFTlog()
        {
            if (this.ftlogfile != null)
                this.ftlogfile.Clear();
            else
                this.ftlogfile = new List<string>();
            this.ftlogfile.Add("timestamp,fx1,fy1,fz1,tx1,ty1,tz1,fx2,fy2,fz2,tx2,ty2,tz2");
            this.logging = true;
        }

        void EndFTlog()
        {
            if (!Directory.Exists("./logs/"))
                Directory.CreateDirectory("./logs/");
            File.WriteAllLines("./logs/" + ftlogname + ".csv", ftlogfile.ToArray());
            ftlogfile.Clear();
            this.logging = false;
        }

        void StartExplog()
        {
            if (this.explogfile != null)
                this.explogfile.Clear();
            else
                this.explogfile = new List<string>();
            this.explogfile.Add("timestamp,fx1,fy1,fz1,tx1,ty1,tz1,fx2,fy2,fz2,tx2,ty2,tz2,xmm,ymm,xpos,ypos");
            this.logging = true;
        }

        void EndEXPlog()
        {
            if (!Directory.Exists("./logs/"))
                Directory.CreateDirectory("./logs/");
            File.WriteAllLines("./logs/" + explogname + ".csv", explogfile.ToArray());
            explogfile.Clear();
            this.logging = false;
        }

        void AddExpLog(DateTime dt, List<double> r1, List<double> r2, double xmm, double ymm, Point pos)
        {
            string l = GetDateTimeString(dt) + ",";
            foreach (double d in r1) { l += d.ToString() + ","; }
            foreach (double d in r2) { l += d.ToString() + ","; }
            l += xmm + "," + ymm + ",";
            l += pos.X + "," + pos.Y + ",";
            explogfile.Add(l);
        }

        void AddExpLog(DateTime dt, string msg)
        {
            string l = GetDateTimeString(dt) + "," + msg + ",";
            explogfile.Add(l);
        }


        void AddFTLog(DateTime dt, List<double> r1, List<double> r2)
        {
            string l = GetDateTimeString(dt) + ",";
            foreach (double d in r1) { l += d.ToString() + ","; } logfile.Add(l);
            foreach (double d in r2) { l += d.ToString() + ","; } logfile.Add(l);
            ftlogfile.Add(l);
        }

        void AddMouseLog(DateTime dt, double x, double y)
        {
            string l = GetDateTimeString(dt) + "," + x.ToString() + "," + y.ToString();
            if(mousefile != null)
                mousefile.Add(l);
        }

        void AddPingLog(double td)
        {
            pingfile.Add(td.ToString());
        }
    
        VistaStyleProgressBar.ProgressBar p;

        Ethernet eth;
        Commands cmd;
        // MjpegDecoder mjp;
        MJPEGStream mjp;
        // Thread mjpThread;
        struct IPCamSettings 
        {
            string defaultURI; string username; string password;
            public IPCamSettings(string defaultURI, string username, string password)
            {
                this.defaultURI = defaultURI;
                this.username = username;
                this.password = password;
            }
            public string DefaultURI { get { return this.defaultURI; } }
            public string Username { get { return this.username; } }
            public string Password { get { return this.password; } }
        };
        IPCamSettings ipcSettings;
        VideoCapture vidCap;
        SubForm sf1, sf2;
        double speed = 20;
        int inc = 25;

        // trackpad
        bool trackpad_en = false;

        // movement vars
        string axis = "X";
        string movement = "Move";

        // robot selected
        int curRobot = 0; // 0: R1, 1: R2

        // joystick
        bool move = true;
        bool move1 = true;
        int selectedRobot = 1; // start with R1 selected

        // connection to ft sensors
        FTsensor ftSensors1;
        FTsensor ftSensors2;
        HapticControl RobotControl = new HapticControl();

        Button[] expButtonArr;

        byte[] ftbuff = new byte[1024];
        Socket ftConnection;
        List<string> ftCmds = new List<string>();

        public MainForm()
        {
            logfile = new List<string>();
            logfile.Add("timestamp,sensor,fx,fy,fz,rx,ry,rz");
            pingfile = new List<string>();
            pingfile.Add("timedif");

            InitializeComponent();

            expButtonArr = new Button[] {exp1Button, exp2Button, exp3Button, exp4Button, exp5Button};

            ipcSettings = new IPCamSettings("http://146.232.146.90/mjpg/video.mjpg", "root", "Password");
            mjp = new MJPEGStream(ipcSettings.DefaultURI);
            mjp.Login = ipcSettings.Username;
            mjp.Password = ipcSettings.Password;
            mjp.NewFrame += new NewFrameEventHandler(mjp_FrameReady);
            /*
             * mjp = new MjpegDecoder();
            mjp.FrameReady += new EventHandler<FrameReadyEventArgs>(mjp_FrameReady);
            mjpThread = new Thread(StartIPCam);
            mjpThread.Priority = ThreadPriority.Lowest;*/
            // vidPanel.Controls.Add(vidCap);

            eth = new Ethernet("146.232.146.111", 80);

            cmd = new Commands(eth);
            try
            {
                servoButtons(servoStatus());
                
                eth.getOutput().SendToBack();
                watchdog.Start();
            }
            catch (Exception e) {
            }

            PopulateFileList();

            // create and start both connections to the ft net boxes
            ftSensors1 = new com.atiia.automation.sensors.FTsensor("146.232.146.253", this, 1);
            ftSensors1.stopAndRestartSingleReadings();
            ftSensors2 = new com.atiia.automation.sensors.FTsensor("146.232.146.254", this, 2);
            ftSensors2.stopAndRestartSingleReadings();

            //string[] rposj = cmd.RPOSJ();
            //foreach (string s in rposj)
            //{
            //    outputBox.AppendText(s + "\n");
            //}

            TrackpadWindow = NewTrackPadWindow();
            //barionet1 = new UDPcommand("146.232.146.217", 12301);

            ftUpdate.Start();

            sf1 = new SubForm("IP Camera");
            sf1.ReturnVideo += new EventHandler(sf_ReturnVideo);

            sf2 = new SubForm("Black Magic Camera");
            sf2.ReturnVideo += new EventHandler(sf2_ReturnVideo);
            sf2.Resize += new EventHandler(sf2_Resize);
            sf2.KeyDown += new KeyEventHandler(sf2_KeyDown);
            
            vidCap = new VideoCapture(vidPanel);
        }



        
        private Form NewTrackPadWindow()
        {
            Form f = new Form();
            f.FormClosing += new FormClosingEventHandler(TrackpadWindow_FormClosing);
            f.KeyPress += new KeyPressEventHandler(TrackpadWindow_KeyPress);
            f.WindowState = FormWindowState.Maximized;
            f.ShowInTaskbar = false;
            f.Visible = false;
            return f;
        }

        public TextBox getFTBox() { return outputBox; }

        // not used
        private void receiveFTcomms()
        {
            initFTcomms();

            while (!Encoding.ASCII.GetString(ftbuff).Equals("exit"))
            {
                string msgIn = "";
                try
                {
                    ftConnection.Receive(ftbuff);
                    msgIn = Encoding.ASCII.GetString(ftbuff);
                    ftCmds.Add(msgIn);
                }
                catch (SocketException)
                {
                    MessageBox.Show("FT connection closed");
                }
            }
        }

        // not used
        private void initFTcomms()
        {
            Socket ftSock = new Socket(AddressFamily.InterNetwork, SocketType.Stream, ProtocolType.IP);
            ftSock.Bind(new IPEndPoint(System.Net.IPAddress.Any, 9999));
            ftSock.Listen(0);
            ftConnection = ftSock.Accept();
        }

        // read command stream files available
        private void PopulateFileList()
        {
            string[] names = Directory.GetFiles(".", "*.csv");
            eth.print(names.Length.ToString());
            foreach (string s in names)
            {
                fileList.Items.Add(new ListViewItem(s));
            }
            fileList.Refresh();
        }

        // read commands file
        public void readCommandsFile(string filename)
        {
            try
            {
                // read all lines from file, then skip first line
                string[] readStringsHeader = System.IO.File.ReadAllLines(filename);
                string[] readStrings = new string[readStringsHeader.Length - 1];
                Array.Copy(readStringsHeader, 1, readStrings, 0, readStrings.Length);

                // update progressbar
                cmdProgress.MaxValue = readStrings.Length;
                cmdProgress.Value = 0;

                // populate command list
                cmdList.Items.Clear();
                foreach (string s in readStrings)
                {
                    cmdList.Items.Add(new ListViewItem(s));
                }
                cmdList.Refresh();

                cmdList.Items[0].Selected = true;
                cmdList.Refresh();

                int index = 0;

                foreach (string s in readStrings)
                {
                    cmdProgress.Value++;
                    if (inputString(s) != 1)
                        break;
                    cmdProgress.Refresh();
                    index++;
                }
            }
            catch (Exception e)
            {
                MessageBox.Show("Could not complete command stream.\n" + e.Message, "Error");
            }
        }

        System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();

        // perform one command from a string
        // see command file for format
        public int inputString(string command)
        {
            // don't need to read posision to ensure timeout does not occur
            watchdog_Tap();

            sw.Reset();
            sw.Start();

            string[] elements = command.Split(',');
            long before = sw.ElapsedMilliseconds;
            if (elements.Length >= 17)
            {
                string newCommand = "2,"; // create new command in correct format
                if (elements[0].Contains("IMOV"))
                {
                    newCommand = newCommand + "IMOV,";
                    // add velocity
                    newCommand = newCommand + elements[1] + ",";
                    // add x, y and z
                    newCommand = newCommand + elements[3] + "," + elements[4] + "," + elements[5] + ",";
                    // add angles and ending zeros
                    newCommand = newCommand + "0,0,0,0";
                    newCommand = newCommand.Replace(" ", "");
                    eth.print(newCommand);
                    elements = newCommand.Split(',');
                }
            }
            selectRobot(int.Parse(elements[0]));

            // handle input command
            if (elements[1].ToUpper().Equals("IMOV"))
            {
                before = sw.ElapsedMilliseconds;
                if (cmd.IMOV(double.Parse(elements[2]), double.Parse(elements[3]), double.Parse(elements[4]), double.Parse(elements[5]), double.Parse(elements[6]), double.Parse(elements[7]), double.Parse(elements[8]), double.Parse(elements[9])) != 0)
                {
                    //UDPtoggle.Checked = false;
                    outputBox.AppendText("Could not complete command sequence\n");
                    return 0;
                }
                //eth.print("move: (" + selectedRobot + ") " + (sw.ElapsedMilliseconds - before).ToString());
            }
            else if (elements[1].ToUpper().Equals("MOVJ"))
            {
                before = sw.ElapsedMilliseconds;
                if (cmd.MOVJ(double.Parse(elements[2]), double.Parse(elements[3]), double.Parse(elements[4]), double.Parse(elements[5]), double.Parse(elements[6]), double.Parse(elements[7]), double.Parse(elements[8]), double.Parse(elements[9])) != 0)
                {
                    MessageBox.Show("Could not complete command sequence");
                    return 0;
                }
                //eth.print("move: (" + selectedRobot + ") " + (sw.ElapsedMilliseconds - before).ToString());
            }
            else if (elements[1].ToUpper().Equals("HOME"))
                homeButton_Click(null, null);
            else if (elements[1].ToUpper().Equals("RESET"))
                resetButton_Click(null, null);
            else if (elements[1].ToUpper().Equals("SVON"))
                cmd.SVON(int.Parse(elements[2]) == 1);

            return 1;
        }

        // GUI button click functions
        private void status_Click(object sender, EventArgs e)
        {
            double[] read = cmd.RSTATS();
            eth.print("status: " + Convert.ToString((int)read[0], 2).PadLeft(8, '0') + " " + Convert.ToString((int)read[1], 2).PadLeft(8, '0'));
        }

        private void startLog(bool check)
        {
            if (check)
                logging = true;
            else
            {
                logging = false;
                if (!Directory.Exists("./logs/"))
                    Directory.CreateDirectory("./logs/"); 
                File.WriteAllLines("./logs/logfile.csv", logfile.ToArray());
                File.WriteAllLines("./logs/pingfile.csv", pingfile.ToArray());
            }
        }

        private bool servoStatus()
        {
            bool stat;
            try
            {
                stat = ((int)cmd.RSTATS()[1] & 64) != 0;
            }
            catch (IndexOutOfRangeException) { return false; }
            return stat;
        }

        private void servoOffButton_Click(object sender, EventArgs e)
        {
            servoToggle(false);
            stopButton_Click(null, null);
        }

        private void servoOnButton_Click(object sender, EventArgs e)
        {
            servoToggle(true);
        }

        private void servoToggle(bool on)
        {
            // startLog(on);
            cmd.SVON(on);
            servoButtons(servoStatus());
        }

        private void servoButtons(bool on)
        {
            if (on)
            {
                servoOnButton.BackColor = Color.Green;
                servoOffButton.BackColor = Color.Gainsboro;
            }
            else
            {
                servoOnButton.BackColor = Color.Gainsboro;
                servoOffButton.BackColor = Color.Red;
            }
        }

        private void Axis_SelectedIndexChanged(object sender, EventArgs e)
        {
            //axis = Axis.Text;
        }

        private void movType_SelectedIndexChanged(object sender, EventArgs e)
        {
            //movement = movType.Text;
        }

        // joystick buttons
        private void plusButton_Click(object sender, EventArgs e)
        {
            eth.print("axis: " + axis);
            eth.print("mov: " + movement);
            if (movement.Equals("Move"))
            {
                if (axis.Equals("X"))
                    cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            }
            else
                if (axis.Equals("X"))
                    cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
        }

        private void minButton_Click(object sender, EventArgs e)
        {
            eth.print("axis: " + axis);
            eth.print("mov: " + movement);
            if (movement.Equals("Move"))
            {
                if (axis.Equals("X"))
                    cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            }
            else
                if (axis.Equals("X"))
                    cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
                else if (axis.Equals("Y"))
                    cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
                else
                    cmd.IMOV(speed, 0, 0, 0, 0, 0, -inc, 0);
        }

        private void alarmsButton_Click(object sender, EventArgs e)
        {
            cmd.RESET().ToString();
            alarmsButton.BackColor = Color.Transparent;
            statusLabel.Text = "Alarm status";
        }

        // select different robots
        private void rob1_Click(object sender, EventArgs e)
        {
            eth.print("Selected Robot 1");
            selectRobot(1);
        }

        private void rob2_Click(object sender, EventArgs e)
        {
            eth.print("Selected Robot 2");
            selectRobot(2);
        }

        // select specified robot, unless it is already selected
        // this function takes roughly 50ms to execute
        private void selectRobot(int rob)
        {
            if (rob == selectedRobot || (rob != 1 && rob != 2)) return; // if robot is already selected, just return and do nothing
            selectedRobot = rob;

            switch (rob)
            {
                case 1:
                    rob1.BackColor = Color.Green;
                    rob2.BackColor = Color.Transparent;
                    break;
                case 2:
                    rob1.BackColor = Color.Transparent;
                    rob2.BackColor = Color.Green;
                    break;
                default:
                    break;
            }
            curRobot = rob - 1;
            cmd.CGROUP(rob);
        }

        // tap watchdog timer to show that it is not needed
        // watchdog timer ensures that ethernet connection does not timeout by sending it routine commands
        private void watchdog_Tap()
        {
            watchdog.Stop();
            watchdog.Start();
        }

        bool mux = false;

        // ensures that ethernet connection does not time out
        private void watchdog_Tick(object sender, EventArgs e)
        {
            // alternate between checking posision and checking alarms
            if (!poll.Checked) return;

            if (mux)
                Posision_Click(null, null);
            else
            {
                bool alarm_found = false;
                string[] read = cmd.RALARM();
                alarmsText.Text = "";
                foreach (string s in read)
                {
                    try
                    {
                        if (int.Parse(s) != 0)
                            alarm_found = true;
                    }
                    catch (FormatException) { }
                    alarmsText.AppendText(s + "\n");

                    // decode alarms
                    try
                    {
                        int recInt = int.Parse(s);
                        switch (recInt)
                        {
                            case 4980:
                                statusLabel.Text = ("Alarm: Destination pulse limit");
                                break;
                            case 4684:
                                statusLabel.Text = ("Alarm: Interpolation invalid");
                                break;
                            case 4416:
                                statusLabel.Text = ("Alarm: Pulse limit");
                                break;
                            default:
                                break;
                        }
                    }
                    catch (FormatException) { }
                }
                if (alarm_found)
                    alarmsButton.BackColor = Color.Red;
                else
                    alarmsButton.BackColor = Color.Transparent;
            }
            mux = !mux;
        }

        // command line commands. not used
        private void commandLine_MouseDown(object sender, MouseEventArgs e)
        {
            if (commandLine.Text.Equals("Enter command here"))
                commandLine.Text = "";
        }

        // input commands directly into GUI
        // not really used
        private void commandLine_KeyPress(object sender, KeyPressEventArgs e)
        {
            if (e.KeyChar == '\r')
            {
                string[] cmdSplit = commandLine.Text.Split(' ');
                if (commandLine.Text.ToUpper().StartsWith("IMOV"))
                {
                    try
                    {
                        statusLabel.Text = "IMOV (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + ")";
                        cmd.IMOV(double.Parse(cmdSplit[1]), double.Parse(cmdSplit[2]), double.Parse(cmdSplit[3]), double.Parse(cmdSplit[4]), double.Parse(cmdSplit[5]), double.Parse(cmdSplit[6]), double.Parse(cmdSplit[7]), double.Parse(cmdSplit[8]));
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
                else if (commandLine.Text.ToUpper().StartsWith("MOVJ"))
                {
                    try
                    {
                        statusLabel.Text = "MOVJ (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + " " + cmdSplit[8] + ")";
                        cmd.MOVJ(double.Parse(cmdSplit[1]), double.Parse(cmdSplit[2]), double.Parse(cmdSplit[3]), double.Parse(cmdSplit[4]), double.Parse(cmdSplit[5]), double.Parse(cmdSplit[6]), double.Parse(cmdSplit[7]), double.Parse(cmdSplit[8]));
                    }
                    catch (IndexOutOfRangeException)
                    {
                        statusLabel.Text = "Incorrect number of arguments";
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
                else if (commandLine.Text.ToUpper().StartsWith("PMOVJ"))
                {
                    try
                    {
                        statusLabel.Text = "PMOVJ (" + cmdSplit[1] + " " + cmdSplit[2] + " " + cmdSplit[3] + " " + cmdSplit[4] + " " + cmdSplit[5] + " " + cmdSplit[6] + " " + cmdSplit[7] + " " + cmdSplit[8] + ")";
                        cmd.PMOVJ(double.Parse(cmdSplit[1]), int.Parse(cmdSplit[2]), int.Parse(cmdSplit[3]), int.Parse(cmdSplit[4]), int.Parse(cmdSplit[5]), int.Parse(cmdSplit[6]), int.Parse(cmdSplit[7]), int.Parse(cmdSplit[8]));
                    }
                    catch (IndexOutOfRangeException)
                    {
                        statusLabel.Text = "Incorrect number of arguments";
                    }
                    catch (FormatException)
                    {
                        statusLabel.Text = "Incorrect format in command";
                    }
                }
            }
            else
            {
                if (commandLine.Text.ToUpper().StartsWith("IMOV"))
                {
                    cmdLabel.Text = "Speed(mm/s)  X(mm)  Y(mm)  Z(mm)  Rx(deg)  Ry(deg)  Rz(deg)  Re(deg)";
                }
                else if (commandLine.Text.ToUpper().StartsWith("MOVJ"))
                {
                    cmdLabel.Text = "Speed(mm/s)  X(mm)  Y(mm)  Z(mm)  Rx(deg)  Ry(deg)  Rz(deg)  Re(deg)";
                }
                else if (commandLine.Text.ToUpper().StartsWith("PMOVJ"))
                {
                    cmdLabel.Text = "Speed(%)  S L U R B T E";
                }
            }
        }

        // gui buttons
        private void label1_Click(object sender, EventArgs e)
        {
        }

        // reset alarms
        private void resetButton_Click(object sender, EventArgs e)
        {
            int old_robot = curRobot;

            for (int a = 1; a <= 2; a++)
            {
                eth.print("RESET");
                selectRobot(a);
                eth.print(cmd.PMOVJ(35, 0, 0, 0, 0, 0, 0, 0).ToString());
                eth.print("DONE");
            }
            selectRobot(old_robot + 1);
        }

        // read current position for both robots
        private void Posision_Click(object sender, EventArgs e)
        {
            int old_robot = curRobot;
            for (int b = 1; b <= 2; b++)
            {
                selectRobot(b);
             
                //DateTime before = DateTime.Now;
                double[] values = cmd.RPOSC();
                //System.TimeSpan timeDif = DateTime.Now - before;
                //AddPingLog(timeDif.TotalMilliseconds);

                for (int a = 0; a < values.Length; a++)
                {
                    PosisionView.Items[a].SubItems[b].Text = values[a].ToString();
                }
            }
            selectRobot(old_robot + 1);
        }

        private void gotoPosision_Click(object sender, EventArgs e)
        {
        }

        // return Right robot to writing position
        private void writePosButton_Click(object sender, EventArgs e)
        {
            selectRobot(2);
            cmd.MOVJ(25, 560, -436, -3, -126, -2, -36, -1.5);
            //if (cmd.PMOVJ(50, 600, 10000, -131900, -25000, 45000, 11300, 15000) > 0)
            //    MessageBox.Show("Could not return to writing position", "Write Position Error");
        }


        private void povButton_Click(object sender, EventArgs e)
        {
            int old_robot = curRobot;

            selectRobot(1);
            cmd.PMOVJ(35, -115716, 78740, -94623, -32968, -122982, -7957, 3447);
            watchdog_Tap();
            selectRobot(2);
            cmd.PMOVJ(35, 7449, 69462, -45878, -19740, -87553, -3881, 7821);

            selectRobot(old_robot + 1);
        }


        // return both robots to home position
        private void homeButton_Click(object sender, EventArgs e)
        {
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            sw.Start();
            eth.print("");
            int old_robot = curRobot;

            selectRobot(1);
            cmd.PMOVJ(35, 5000, 25000, -75000, -2000, -50000, 0, 5000);
            watchdog_Tap();
            selectRobot(2);
            cmd.PMOVJ(35, 5000, 25000, -75000, -2000, -50000, 0, 5000);

            selectRobot(old_robot + 1);
        }

        private void Trackpad_Click(object sender, EventArgs e)
        {
            /*
            homeButton_Click(null, null);
            trackpad_en = !trackpad_en;
            if (trackpad_en)
            {
                TrackpadWindow.Controls.Add(Trackpad);
                TrackpadWindow.Visible = true;
                TrackpadWindow.TopMost = true;
                Trackpad.Dock = DockStyle.Fill;
                Trackpad.BackColor = Color.Green;

                // move both robots to start position

                selectRobot(2);
                eth.print(cmd.MOVJ(50, 550, -350, 250, 90, 0, 90, 0).ToString());
                if (bothRobots)
                {
                    selectRobot(1);
                    eth.print(cmd.MOVJ(50, 550, 350, 250, 90, 0, 90, 0).ToString());
                }
            }*/
            /*
             * else
            {
            }*/
        }

        void TrackpadWindow_FormClosing(object sender, FormClosingEventArgs e)
        {
            trackpad_en = false;

            // TrackpadWindow.Controls.Clear();
            // TrackpadWindow.Visible = false;
            Trackpad.Dock = DockStyle.None;
            Trackpad.BackColor = Color.Maroon;
            splitter1.Controls.Add(Trackpad);

            if (!Directory.Exists("./logs/"))
                Directory.CreateDirectory("./logs/");
            try
            {
                File.WriteAllLines("./logs/mouselog.csv", mousefile.ToArray());
            }
            catch (Exception) { MessageBox.Show("Log file in use", "Mouse error"); }
        }


        void TrackpadWindow_KeyPress(object sender, KeyPressEventArgs e)
        {
            // close window on escape
            if ((int)e.KeyChar == 27)
            {
                trackpad_en = false;

                TrackpadWindow.Controls.Clear();
                TrackpadWindow.Visible = false;
                Trackpad.Dock = DockStyle.None;
                Trackpad.BackColor = Color.Maroon;
                splitter1.Controls.Add(Trackpad);

                if (!Directory.Exists("./logs/"))
                    Directory.CreateDirectory("./logs/");
                try
                {
                    File.WriteAllLines("./logs/mouselog.csv", mousefile.ToArray());
                }
                catch (Exception) { MessageBox.Show("Log file in use", "Mouse error"); }
            }
        }

        private void Trackpad_PreviewKeyDown(object sender, PreviewKeyDownEventArgs e)
        {
            MessageBox.Show("key: " + e.KeyData + " " + Keys.Escape);
            if (e.KeyData == Keys.Escape)
            {
                TrackpadWindow.Controls.Clear();
                TrackpadWindow.Visible = false;
                Trackpad.Dock = DockStyle.None;

                Trackpad.BackColor = Color.Maroon;
            }
        }

        bool hold_on = false;

        // toggle hold button
        public void hold(bool on)
        {
            hold_on = on;
            if (on)
            {
                cmd.HOLD(1);
                holdButton.BackColor = Color.Red;
            }
            else
            {
                cmd.HOLD(0);
                holdButton.BackColor = Color.Transparent;
            }
        }

        private void holdButton_Click(object sender, EventArgs e)
        {
            hold(!hold_on);
        }

        // random movement
        // not used
        private void button1_Click(object sender, EventArgs e)
        {
            int i = 50;
            int s = 100;
            System.Diagnostics.Stopwatch sw = new System.Diagnostics.Stopwatch();
            long oldTime = 0;

            oldTime = 0;
            sw.Reset();
            sw.Start();

            selectRobot(1);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(2);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(1);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, -i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            selectRobot(2);
            eth.print("S " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
            cmd.IMOV(s, -i, 0, 0, 0, 0, 0, 0);
            eth.print("M " + sw.ElapsedMilliseconds + " (" + (sw.ElapsedMilliseconds - oldTime) + ")");
            oldTime = sw.ElapsedMilliseconds;
        }

        private void startButton_Click(object sender, EventArgs e)
        {
            cmd.START();
        }

        // press button to move robot
        // numpad buttons control R2 and WASD control R1
        // 5 and S switch between rotate and move
        private void tabControl1_KeyPress(object sender, KeyPressEventArgs e)
        {
            //if (!keypadBox.Checked) return; // dont respond to movement commands if this box is not checked
                
            //double inc = 1;
            //int speed = 20;
            //switch (e.KeyChar)
            //{
            //    // keyboard
            //    case 'z':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, -inc, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, inc, inc, 0);
            //        break;
            //    case 'x':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
            //        break;
            //    case 'c':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, inc, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, inc, 0, 0, inc, -inc, 0);
            //        break;
            //    case 'a':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
            //        break;
            //    case 's':
            //        selectRobot(2);
            //        move1 = !move1;
            //        if (move1)
            //        {
            //            eth.print("Switch to move");
            //            inc = 2.5;
            //        }
            //        else
            //        {
            //            eth.print("Switch to rotate");
            //            inc = 0.5;
            //        }
            //        break;
            //    case 'd':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, inc, 0, 0, 0, -inc, 0);
            //        break;
            //    case 'q':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, -inc, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, inc, 0);
            //        break;
            //    case 'w':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
            //        break;
            //    case 'e':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, 0, inc, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, -inc, 0);
            //        break;
            //    case 'f':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
            //        break;
            //    case 'r':
            //        selectRobot(2);
            //        if (move1)
            //            cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
            //        break;
            //    case '.':
            //        alarmsButton_Click(null, null);
            //        break;

            //    // numpad
            //    case '1':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, -inc, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, inc, inc, 0);
            //        break;
            //    case '2':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, inc, 0, 0);
            //        break;
            //    case '3':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, inc, -inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, inc, 0, 0, inc, -inc, 0);
            //        break;
            //    case '4':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, -inc, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, 0, inc, 0);
            //        break;
            //    case '5':
            //        selectRobot(1);
            //        move = !move;
            //        if (move)
            //        {
            //            eth.print("Switch to move");
            //            inc = 2.5;
            //        }
            //        else
            //        {
            //            eth.print("Switch to rotate");
            //            inc = 0.5;
            //        }
            //        break;
            //    case '6':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, inc, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, inc, 0, 0, 0, -inc, 0);
            //        break;
            //    case '7':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, -inc, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, inc, 0);
            //        break;
            //    case '8':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, 0, 0);
            //        break;
            //    case '9':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, 0, inc, inc, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, 0, -inc, -inc, 0);
            //        break;
            //    case '+':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, inc, 0, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, inc, 0, 0, 0);
            //        break;
            //    case '-':
            //        selectRobot(1);
            //        if (move)
            //            cmd.IMOV(speed, -inc, 0, 0, 0, 0, 0, 0);
            //        else
            //            cmd.IMOV(speed, 0, 0, 0, -inc, 0, 0, 0);
            //        break;
            //    default:
            //        break;
            //}
        }

        private void num9_Click(object sender, EventArgs e)
        {
        }

        // movement button
        private void button3_Click(object sender, EventArgs e)
        {
            int speed = 1000;
            homeButton_Click(null, null);
            for (int x = 0; x < 10; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, 10, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, 10, 0, 0, 0, 0);
            }
            for (int x = 0; x < 10; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, -10, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, -10, 0, 0, 0, 0);
            }
        }

        // movement button
        private void button4_Click(object sender, EventArgs e)
        {
            int speed = 1000;
            double inc = 2.5;
            homeButton_Click(null, null);
            for (int x = 0; x < 50; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, inc, 0, 0, 0, 0);
            }
            for (int x = 0; x < 50; x++)
            {
                selectRobot(1);
                cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
                selectRobot(2);
                cmd.IMOV(speed, 0, 0, -inc, 0, 0, 0, 0);
            }
        }

        // read commands file
        private void cmdFileButton_Click(object sender, EventArgs e)
        {
            readCommandsFile("Commands.csv");
        }

        // do the selected commands file
        private void fileList_ItemActivate(object sender, EventArgs e)
        {
            string filename = fileList.SelectedItems[0].Text;
            readCommandsFile(filename);
        }

        private void MainForm_FormClosing(object sender, FormClosingEventArgs e)
        {
            vidCap.Close();
            mjp.Stop();

            cmd.SVON(false);

            ftSensors2.stop_reading();
            ftSensors1.stop_reading();

            ftUpdate.Enabled = false;
            ftUpdate.Stop();
            // Environment.Exit(0);
        }

        /**Displays new F/T data using InvokeLater, so it's thread-safe
        *@param displayRDT - the NetFTRDTPacket containing the F/T data to
        *display
        */

        double[] ft1RealReading, ft2RealReading;
        bool newValues = false;

        public void updateFTData(NetFTRDTPacket displayRDT, int NUM_FT_AXES, double[] m_daftCountsPerUnit, int sensor_id)
        {
            try
            {
                //List<double> ld;
                
                if (sensor_id == 1)
                {
                    ftSensors1.setRDTPacket(displayRDT);
                    //string str = "";
                    //str += ("0x" + String.Format("%08x", (int)displayRDT.getStatus()) + "\n");
                    //str += ("" + displayRDT.getRDTSequence() + "\n");
                    //str += ("" + displayRDT.getFTSequence() + "\n");
                    int[] ftCountsReading = displayRDT.getFTArray();
                    ft1RealReading = new double[NUM_FT_AXES];
                    int i;
                    for (i = 0; i < NUM_FT_AXES; i++)
                    {
                        /*display the numbers*/
                        ft1RealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];
                        //str += (ft1RealReading[i].ToString("#.000") + "\n");
                    }
                    //ld = new List<double>(ft1RealReading);
                }
                else
                {
                    ftSensors2.setRDTPacket(displayRDT);
                    //string str = "";
                    //str += ("0x" + String.Format("%08x", (int)displayRDT.getStatus()) + "\n");
                    //str += ("" + displayRDT.getRDTSequence() + "\n");
                    //str += ("" + displayRDT.getFTSequence() + "\n");
                    int[] ftCountsReading = displayRDT.getFTArray();
                    ft2RealReading = new double[NUM_FT_AXES];
                    int i;
                    for (i = 0; i < NUM_FT_AXES; i++)
                    {
                        /*display the numbers*/
                        ft2RealReading[i] = ftCountsReading[i] / m_daftCountsPerUnit[i];
                        //str += (ft2RealReading[i].ToString("#.000") + "\n");
                    }
                    //ld = new List<double>(ft2RealReading);
                }
                newValues = true;

                //if (logging)
                //    AddFTLog(DateTime.Now, sensor_id, ld);
            }
            catch (System.ComponentModel.Win32Exception)
            {
                // ignore, since this only happens when program is closing
            }
        }

        private double[] Subtract(double[] a, double[] b)
        {
            if(a.Length != b.Length) return null;
            double[] c = new double[a.Length];
            for (int i = 0; i < a.Length; i++) { c[i] = a[i] - b[i]; }
            return c;
        }

        // timer ticks that synchronizes data read from sensors
        // set timeout in designer
        // if new values has been received, update gui elements and relay movement commands

        bool showing_msg = false;
        List<double> readings1, readings2;
        private void FTupdate_Tick(object sender, EventArgs e)
        {
            // wait 50 ms at the end if not moving, otherwise the gui tries to update too quickly
            if (newValues)
            {
                ftActivity.Checked = !ftActivity.Checked;

                // show data other than ft readings
                status1.Text = ((int)ftSensors1.getRDTPacket().getStatus()).ToString();
                RDT1.Text = ftSensors1.getRDTPacket().getRDTSequence().ToString();
                FTseq1.Text = ftSensors1.getRDTPacket().getFTSequence().ToString();

                readings1 = new List<double>(Subtract(ft1RealReading, bias1));
                readings2 = new List<double>(Subtract(ft2RealReading, bias2));

                // show ft readings
                update_gui_elements(readings1[0], Fx1, Fx1progressBar);
                update_gui_elements(readings1[1], Fy1, Fy1progressBar);
                update_gui_elements(readings1[2], Fz1, Fz1progressBar);
                update_gui_elements(readings1[3], Tx1, Tx1progressBar);
                update_gui_elements(readings1[4], Ty1, Ty1progressBar);
                update_gui_elements(readings1[5], Tz1, Tz1progressBar);

                status2.Text = ((int)ftSensors2.getRDTPacket().getStatus()).ToString();
                RDT2.Text = ftSensors2.getRDTPacket().getRDTSequence().ToString();
                FTseq2.Text = ftSensors2.getRDTPacket().getFTSequence().ToString();

                update_gui_elements(readings2[0], Fx2, Fx2progressBar);
                update_gui_elements(readings2[1], Fy2, Fy2progressBar);
                update_gui_elements(readings2[2], Fz2, Fz2progressBar);
                update_gui_elements(readings2[3], Tx2, Tx2progressBar);
                update_gui_elements(readings2[4], Ty2, Ty2progressBar);
                update_gui_elements(readings2[5], Tz2, Tz2progressBar);

                if (current_act == Activity.FT && current_ft != FTactivity.None)
                {
                    //sw.Reset();
                    //sw.Start();
                    //bool serv = !servoStatus();
                    //tlabel.Text = sw.ElapsedMilliseconds.ToString();
                        
                    //if (serv)
                    //{
                    //    if (!showing_msg)
                    //    {
                    //        showing_msg = true;
                    //        System.Windows.Forms.MessageBox.Show("Turn on servo");
                    //        showing_msg = false;
                    //        current_act = Activity.None ;
                    //        ftActButtons(current_ft);
                    //    }
                    //}
                    //else
                    //{
                        if (logging)
                            AddFTLog(DateTime.Now, readings1, readings2);
                        
                        // move robots based on ft data
                        RobotControl.BiasFT(bias1, 1);
                        RobotControl.BiasFT(bias2, 2);
                        FT cmdFT = RobotControl.Inc_Control_Robot();
                        
                        cmdFT.RoundFT();
                        if (cmdFT.time == -1) // no movement
                        {
                            Thread.Sleep(25);
                            return;
                        }

                        int arm = GetArmNum(current_arm);

                        //string str;
                        ////if (arm == 2)
                        //str = cmdFT.Fx + " " + cmdFT.Fy + " " + cmdFT.Fz + " " + cmdFT.Tx + " " + cmdFT.Ty + " " + cmdFT.Tz + " ";
                        ////else
                        ////    str = cmdFT.Fx + " " + cmdFT.Fy + " " + cmdFT.Fz + " " + cmdFT.Tx + " " + cmdFT.Ty + " " + cmdFT.Tz + " ";
                        //outputBox.AppendText(str + "\n");

                        //string udpCmd = HapticControl.RobotCmnd(50, cmdFT);
                        //outputBox.AppendText("UDP: " + udpCmd + "\n");
                        //barionet1.UDPSend(udpCmd);
                        //Thread.Sleep(1500);
                        
                        // move SDA 10
                        outputBox.AppendText(arm + ",IMOV," + (largest(cmdFT)) * hapScale + "," + cmdFT.Fz * hapScale + "," + -cmdFT.Fx * hapScale + "," + -cmdFT.Fy * hapScale + "," + -cmdFT.Tz + "," + -cmdFT.Tx + "," + -cmdFT.Ty + ",0");
                        inputString(arm + ",IMOV," + (largest(cmdFT)) * hapScale + "," + cmdFT.Fz * hapScale + "," + -cmdFT.Fx * hapScale + "," + -cmdFT.Fy * hapScale + "," + -cmdFT.Tz + "," + -cmdFT.Tx + "," + -cmdFT.Ty + ",0");
                        
                        // inputString(arm + ",IMOV," + (largest(cmdFT)) + "," + cmdFT.Fz + "," + -cmdFT.Fx + "," + -cmdFT.Fy + "," + -cmdFT.Tz + "," + -cmdFT.Tx + "," + -cmdFT.Ty + ",0");
                        return;
                    }
                //}
                
                // Thread.Sleep(50);

                
                /*
                if (UDPtoggle.Checked)
                {
                    if (!servoStatus())
                    {
                        if (!showing_msg)
                        {
                            showing_msg = true;
                            UDPtoggle.Checked = false;
                            System.Windows.Forms.MessageBox.Show("Turn on servo");
                            showing_msg = false;
                        }
                    }
                    else
                    {

                        // move robots based on ft data
                        RobotControl.BiasFT(bias1, 1);
                        RobotControl.BiasFT(bias2, 2);
                        FT cmdFT = RobotControl.Inc_Control_Robot();

                        cmdFT.RoundFT();
                        if (cmdFT.time == -1) // no movement
                        {
                            Thread.Sleep(50);
                            return;
                        }

                        int arm = udpSelect.SelectedIndex + 1;
                        
                        string str ;
                        if(arm == 2)
                            str = cmdFT.Fx + " " + cmdFT.Fy + " " + cmdFT.Fz + " " + cmdFT.Tx + " " + cmdFT.Ty + " " + cmdFT.Tz + " ";
                        else
                            str = cmdFT.Fx + " " + cmdFT.Fy + " " + cmdFT.Fz + " " + cmdFT.Tx + " " + cmdFT.Ty + " " + cmdFT.Tz + " ";
                        outputBox.AppendText(str + "\n");

                        //string udpCmd = HapticControl.RobotCmnd(50, cmdFT);
                        //outputBox.AppendText("UDP: " + udpCmd + "\n");
                        //barionet1.UDPSend(udpCmd);
                        //Thread.Sleep(1500);
                        inputString(arm + ",IMOV," + (5 * largest(cmdFT)) + "," + cmdFT.Fz + "," + -cmdFT.Fx + "," + -cmdFT.Fy + "," + cmdFT.Tz + "," + cmdFT.Tx + "," + cmdFT.Ty + ",0");
                        return;
                    }
                }
                Thread.Sleep(50);
                 * */
            }
        }

        // simple algorithm to calculate largest movement
        private double largest(FT ftin)
        {
            double large = Math.Max(Math.Abs(ftin.Fx), Math.Abs(ftin.Fy));
            large = Math.Max(Math.Abs(ftin.Fz), large);
            large = Math.Max(Math.Abs(ftin.Tx), large);
            large = Math.Max(Math.Abs(ftin.Ty), large);
            large = Math.Max(Math.Abs(ftin.Tz), large);
            return Math.Min(5 * large, double.Parse(maxBox.Text));
        }

        // update GUI elements with data
        private Color POSITIVE_COLOR = Color.Red, NEGATIVE_COLOR = Color.Lime;

        // just for drawing progress bars
        int max_force = 10000;
        int max_torque = 1000;
        private void update_gui_elements(double dataIn, TextBox tb, VistaStyleProgressBar.ProgressBar pb)
        {
            tb.Text = String.Format("{0:0.00}", dataIn);
            if (dataIn < 0)
            {
                pb.StartColor = NEGATIVE_COLOR;
                pb.EndColor = NEGATIVE_COLOR;
            }
            else
            {
                pb.StartColor = POSITIVE_COLOR;
                pb.EndColor = POSITIVE_COLOR;
            }


            // check that value falls in limits:
            int newVal = (int)(Math.Abs(dataIn) * 1000);
            if (pb.Tag.Equals("Force"))
            {
                if (newVal > max_force)
                    max_force = newVal;
                pb.MaxValue = max_force;
            }
            else if (pb.Tag.Equals("Torque"))
            {
                if (newVal > max_torque)
                    max_torque = newVal;
                pb.MaxValue = max_torque;
            }
            pb.Value = newVal;
        }

        internal bool IsSending()
        {
            return current_act == Activity.FT;
        }

        // bias values for sensors 1 and 2
        double[] bias1 = new double[6];
        double[] bias2 = new double[6];
        // bias sensors so current position is considered "zero ft"
        private void biasButton_Click(object sender, EventArgs e)
        {
            Array.Copy(ft1RealReading, bias1, ft1RealReading.Length);
            Array.Copy(ft2RealReading, bias2, ft2RealReading.Length);
        }

        public void UpdateRobot(NetFTRDTPacket RDT, int sensor_id)
        {
            if (current_ft == FTactivity.None) return;
            if (sensor_id == 1)
                RobotControl.UpdateSensor1(RDT);
            else if (sensor_id == 2)
                RobotControl.UpdateSensor2(RDT);
        }

        Point trackpad_start = new Point();
        bool trackpad_down = false;
        private void Trackpad_MouseDown(object sender, MouseEventArgs e)
        {
            if (trackpad_en)
            {
                trackpad_down = true;
                AddExpLog(DateTime.Now, "Begin");
            } 
            trackpad_start.X = e.X;
            trackpad_start.Y = e.Y;
        }

        // pick up new movement from the trackpad and send new position to robot
        private void Trackpad_MouseMove(object sender, MouseEventArgs e)
        {
            double factor = 150/1920.0;
            // if (trackpad_en)
            if (trackpad_down)
            {
                current_mouse_pos.X = e.X;
                current_mouse_pos.Y = e.Y;
                eth.print(((e.X - trackpad_start.X) * factor).ToString());
                watchdog_Tap();
                selectRobot(2);
                eth.print(cmd.IMOV(30, -(e.X - trackpad_start.X) * factor, (e.Y - trackpad_start.Y) * factor, 0, 0, 0, 0, 0).ToString());
                // AddMouseLog(DateTime.Now, (e.X) * factor, (e.Y) * factor);
                trackpad_start.X = e.X;
                trackpad_start.Y = e.Y;

                double[] values = cmd.RPOSC();
                AddExpLog(DateTime.Now, readings1, readings2, values[0], values[1], new Point(e.X, e.Y));
            }
        }

        private void Trackpad_MouseUp(object sender, MouseEventArgs e)
        {
            trackpad_down = false;
            if(trackpad_en)
                AddExpLog(DateTime.Now, "End");
            /*
            if (!trackpad_start.IsEmpty)
            {
                int x, y;
                x = e.X - trackpad_start.X;
                y = e.Y - trackpad_start.Y;

                if (x == 0 && y == 0)
                {
                    trackpad_en = !trackpad_en;
                    if (trackpad_en)
                    {
                        if (!servoBox.Checked)
                            MessageBox.Show("Servo off");
                        else
                        {
                            TrackpadWindow.Controls.Add(Trackpad);
                            TrackpadWindow.Visible = true;
                            TrackpadWindow.TopMost = true;
                            Trackpad.Dock = DockStyle.Fill;
                            Trackpad.BackColor = Color.Green;

                            // move both robots to start position
                            
                            selectRobot(2);
                            /*
                            eth.print(cmd.MOVJ(30, 550, -350, 250, 90, 0, 90, 0).ToString());
                            if (bothRobots)
                            {
                                selectRobot(1);
                                eth.print(cmd.MOVJ(30, 550, 350, 250, 90, 0, 90, 0).ToString());
                            }*//*
                        }
                    }
                }
                else
                {
                    if (trackpad_en)
                    {
                        if (bothRobots)
                            selectRobot(2);
                        eth.print((-350 + (double)e.X / 5.0 - 50).ToString());
                        watchdog_Tap();
                        eth.print(cmd.MOVJ(50, 550, -350 + (double)x / 5.0 - 50, 250 - (y / 5.0 - 50), 90, 0, 90, 0).ToString());
                        if (bothRobots)
                        {
                            selectRobot(1);
                            watchdog_Tap();
                            eth.print(cmd.MOVJ(50, 550, 350 - x / 5.0 - 50, 250 - (y / 5.0 - 50), 90, 0, 90, 0).ToString());
                        }
                    }
                }*/
    
        }

        private void Trackpad_DoubleClick(object sender, EventArgs e)
        {


        }

        #region Video Stuff
        bool ipvidFS = false;
        bool hdmiShow = false;
        bool hdmiFS = false;
        
        Thread mjp_fs_thread;
        bool mjp_fs_ready = false;
        Bitmap mjp_fs_img = null;

        void mjp_fs_draw()
        {
            while (ipvidFS)
            {
                if (mjp_fs_ready)
                {
                    sf1.Img = mjp_fs_img;
                    mjp_fs_ready = false;
                }
            }
        }

        void mjp_FrameReady(object sender, NewFrameEventArgs e)
        {
            if (ipvidFS)
            {
                sf1.Img = e.Frame;
                //mjp_fs_img = e.Frame;
                //mjp_fs_ready = true;
            }
            else
                ipcamPanel.BackgroundImage = e.Frame;
        }

        private void ipcFSButton_Click(object sender, EventArgs e)
        {
            if (!ipvidFS)
            {
                sf1.Trigger();
                ipvidFS = true;
                //mjp_fs_thread = new Thread(mjp_fs_draw);
                //mjp_fs_thread.Priority = ThreadPriority.Lowest;
                //mjp_fs_thread.Start();
            }
        }

        private void ipcamPanel_Click(object sender, EventArgs e)
        {
        }

        void sf_ReturnVideo(object sender, EventArgs e)
        {
            this.ipcamPanel.Dock = DockStyle.None;
            this.camPanel.Controls.Add(this.ipcamPanel);
            ipvidFS = false;
        }

        private void hdmiFSbutton_Click(object sender, EventArgs e)
        {
            if (!hdmiShow)
            {
                this.sf2.Trigger();
                this.vidCap.SetOwner(this.sf2);
                hdmiShow = true;
                this.vidCap.SetSize(this.sf2.ClientRectangle.Width, this.sf2.ClientRectangle.Height);
            }
        }

        void sf2_ReturnVideo(object sender, EventArgs e)
        {
            this.vidPanel.Dock = DockStyle.None;
            this.camPanel.Controls.Add(this.vidPanel);
            this.vidCap.SetOwner(this.vidPanel);
            this.vidCap.SetSize(this.vidPanel.Width, this.vidPanel.Height);
            hdmiShow = false;
        }

        void sf2_Resize(object sender, EventArgs e)
        {
            if(hdmiFS)
                this.vidCap.SetSize(sf2.ClientRectangle.Width, sf2.ClientRectangle.Height);
            else if (hdmiShow)
                this.vidCap.SetSize(sf2.ClientRectangle.Width, sf2.ClientRectangle.Height);
            
        }

        void sf2_KeyDown(object sender, KeyEventArgs e)
        {
            if (e.KeyCode == Keys.Enter && e.Modifiers == Keys.Alt)
            {
                hdmiFS = !hdmiFS;
                this.vidCap.Fullscreen(hdmiFS);
            }
        }
       
        private void vidPanel_Click(object sender, EventArgs e)
        {
        }

        private void showCamPanelBox_CheckedChanged(object sender, EventArgs e)
        {
            bool show = showCamPanelBox.Checked;

            if (show)
            {
                vidCap.Play();
                mjp.Start();
                tabControl.Location = new Point(tabControl.Location.X + camPanel.Width, 0);
                tabControl.Size = new Size(tabControl.Size.Width - camPanel.Width, tabControl.Size.Height);
            }
            else
            {
                vidCap.Stop();
                mjp.Stop();
                tabControl.Location = new Point(tabControl.Location.X - camPanel.Width, 0);
                tabControl.Size = new Size(tabControl.Size.Width + camPanel.Width, tabControl.Size.Height);
                sf1.Visible = false;
                sf2.Visible = false;
                hdmiFS = false;
                ipvidFS = false;
                hdmiShow = false;
            }

            camPanel.Visible = show;
        }

        private void StartIPCam()
        {
            //mjp.ParseStream(ipcSettings.DefaultURI, ipcSettings.Username, ipcSettings.Password);
        }
        #endregion

        #region Experiment tab
        private void expButton_click(object sender, EventArgs e)
        {
            this.current_exp = int.Parse(((Button)(sender)).Tag.ToString());
            foreach (Button b in expButtonArr)
                b.BackColor = Color.Gainsboro;
            expButtonArr[this.current_exp - 1].BackColor = Color.Green;
        }

        private void hapticExpButton_Click(object sender, EventArgs e)
        {
            current_exp_type = FTactivity.Haptic;
            expButtons(current_exp_type);
        }

        private void joystickExpButton_Click(object sender, EventArgs e)
        {
            current_exp_type = FTactivity.Joystick;
            
            expButtons(current_exp_type);
        }

        private void expButtons(FTactivity act)
        {
            if (act == FTactivity.Haptic)
            {
                hapticExpButton.BackColor = Color.Green;
                joystickExpButton.BackColor = Color.Gainsboro;
            }
            else if (act == FTactivity.Joystick)
            {
                hapticExpButton.BackColor = Color.Gainsboro;
                joystickExpButton.BackColor = Color.Green;
            }
            else
            {
                hapticExpButton.BackColor = Color.Gainsboro;
                joystickExpButton.BackColor = Color.Gainsboro;
            }
        }

        private void startExpButton_Click(object sender, EventArgs e)
        {
            if (current_exp_type == FTactivity.None)
            {
                MessageBox.Show("Please select experiment type.\n(Joystick/Haptic)");
            }
            else
            {
                if (!trackpad_en)
                {
                    trackpad_en = true;
                }

                if (trackpad_en)
                {
                    if (!servoStatus())
                    {
                        MessageBox.Show("Servo is off");
                        trackpad_en = false;
                    }
                    else
                    {
                        selectRobot(1);

                        if (TrackpadWindow.IsDisposed)
                            TrackpadWindow = NewTrackPadWindow();

                        mousefile = new List<string>();
                        mousefile.Add("dt,x,y");
                        Trackpad.BackColor = Color.Green;

                        explogname = getExplogname(current_exp_type, current_exp);
                        current_act = Activity.Experiment;
                        this.StartExplog();
                    }
                }
            }
        }

        private void endExpButton_Click(object sender, EventArgs e)
        {
            if (logging)
                this.EndEXPlog();

            current_act = Activity.None;
            current_exp_type = FTactivity.None;
            expButtons(current_exp_type);

            current_exp = 0;
            foreach (Button b in expButtonArr)
                b.BackColor = Color.Gainsboro;

            TrackpadWindow.Controls.Clear();
            TrackpadWindow.Visible = false;
            Trackpad.Dock = DockStyle.None;

            Trackpad.BackColor = Color.Maroon;

        }

        string getExplogname(FTactivity fta, int expnum)
        {
            string str = "exp_";
            if (fta == FTactivity.Haptic)
                str += "haptic_";
            else if (fta == FTactivity.Joystick)
                str += "joystick_";
            str += expnum + "_";
            str += DateTime.Now.ToLongDateString() + "_" + DateTime.Now.ToLongTimeString() + "_" + explognum++;
            return str.Replace(':', '.');
        }

        private void trackpadFSbutton_Click(object sender, EventArgs e)
        {
            if (trackpad_en)
            {
                TrackpadWindow.WindowState = FormWindowState.Maximized;
                TrackpadWindow.Controls.Add(Trackpad);
                TrackpadWindow.Visible = true;
                TrackpadWindow.TopMost = true;
                Trackpad.Dock = DockStyle.Fill;
            }
        }

        #endregion

        #region FT sensor tab
        private void leftButton_Click(object sender, EventArgs e)
        {
            current_arm = SDA10Arm.Left;
            armsButtons(current_arm);
        }

        private void rightButton_Click(object sender, EventArgs e)
        {
            current_arm = SDA10Arm.Right;
            armsButtons(current_arm);
        }

        private void armsButtons(SDA10Arm active)
        {
            if (active == SDA10Arm.Left)
            {
                leftButton.BackColor = Color.Green;
                rightButton.BackColor = Color.Gainsboro;
            }
            else
            {
                rightButton.BackColor = Color.Green;
                leftButton.BackColor = Color.Gainsboro;
            }
        }

        private int GetArmNum(SDA10Arm arm) { if (arm == SDA10Arm.Right) return 2; else return 1; }

        private void joystickButton_Click(object sender, EventArgs e)
        {
            if (servoStatus())
            {
                current_act = Activity.FT;
                current_ft = FTactivity.Joystick;
                ftlogname = getFTlogname(current_ft);
                this.StartFTlog();
                ftActButtons(current_ft);
            }
            else
                if (!showing_msg)
                    MessageBox.Show("Please turn on servo first", "Servo error");
        }

        private void stopButton_Click(object sender, EventArgs e)
        {
            if (logging)
                this.EndFTlog();
            current_ft = FTactivity.None;
            ftActButtons(current_ft);
        }

        string getFTlogname(FTactivity fta)
        {
            string str = "";
            if (fta == FTactivity.Haptic)
                str += "haptic_";
            else if (fta == FTactivity.Joystick)
                str += "joystick_";
            str += DateTime.Now.ToLongDateString() + "_" + DateTime.Now.ToLongTimeString() + "_" + ftlognum++ + "_Robot_" + this.GetArmNum(this.current_arm);
            return str.Replace(':', '.');
        }

        private void hapticButton_Click(object sender, EventArgs e)
        {
            if (servoStatus())
            {
                current_act = Activity.FT;
                current_ft = FTactivity.Haptic;
                ftlogname = getFTlogname(current_ft);
                this.StartFTlog();
                ftActButtons(current_ft);
            }
            else
                if (!showing_msg)
                    MessageBox.Show("Please turn on servo first", "Servo error");
        }

        private void ftActButtons(FTactivity act)
        {
            if (act == FTactivity.None)
            {
                stopButton.BackColor = Color.Red;
                hapticButton.BackColor = Color.Gainsboro;
                joystickButton.BackColor = Color.Gainsboro;
            }
            else if (act == FTactivity.Haptic)
            {
                stopButton.BackColor = Color.Gainsboro;
                hapticButton.BackColor = Color.Green;
                joystickButton.BackColor = Color.Gainsboro;
            }
            else
            {
                stopButton.BackColor = Color.Gainsboro;
                hapticButton.BackColor = Color.Gainsboro;
                joystickButton.BackColor = Color.Green;
            }

        }
        #endregion

        private void rotButton_Click(object sender, EventArgs e)
        {
            if (cmd.AllowRotation)
                rotButton.BackColor = Color.Red;
            else
                rotButton.BackColor = Color.Green;
            cmd.AllowRotation = !cmd.AllowRotation;
        }

        double hapScale = 1.0;
        private void hapticScale_ValueChanged(object sender, EventArgs e)
        {
            hapScale = hapticScale.Value / 100.0;
            scaleLabel.Text = (hapScale * 100.0).ToString() + " %" ;
        }

    }
}